/**
 * @file command_executor_motor.cpp
 * @brief ESP32沙画机电机控制功能实现
 * 
 * 电机功能:
 * - 电机启用/禁用
 * - 归零操作
 * - 位置移动
 * - 参数设置
 * - 自动管理
 */

#include "command_executor.h"
#include "command_executor_debug.h"

// ==================== 电机控制命令实现 ====================

int CommandExecutor::executeHomeCommand() {
    // 使用统一的前置检查
    if (!preExecuteCheck("home")) {
        return 1;
    }
    
    Serial.println("[HOME] 执行归零命令");
    int result = performHomingOperation();
    
    // 使用统一的后置处理
    postExecuteProcess("home", result);
    return result;
}

// 统一的电机操作函数
int CommandExecutor::executeMotorOperation(const char* operation, bool enableOperation) {
    if (!motionController) {
        handleError(1, "MotionController未初始化");
        return 1;
    }
    
    if (enableOperation) {
        motionController->enableMotors();
        Serial.printf("[MOTOR] 电机已%s\n", operation);
    } else {
        motionController->disableMotors();
        Serial.printf("[MOTOR] 电机已%s\n", operation);
    }
    
    return 0;
}

int CommandExecutor::enableMotors() {
    return executeMotorOperation("启用", true);
}

int CommandExecutor::disableMotors() {
    return executeMotorOperation("禁用", false);
}

int CommandExecutor::moveTo(float theta, float rho) {
    if (!canExecuteMotorCommand("moveTo")) {
        return 1;
    }
    
    if (!motionController) {
        return 2;
    }
    
    motionController->moveTo(theta, rho);
    Serial.printf("[MOTOR] 移动到位置: theta=%.3f, rho=%.3f\n", theta, rho);
    return 0;
}

int CommandExecutor::moveToSteps(long thetaSteps, long rhoSteps) {
    if (!canExecuteMotorCommand("moveToSteps")) {
        return 1;
    }
    
    if (!motionController) {
        return 2;
    }
    
    motionController->moveToSteps(thetaSteps, rhoSteps);
    Serial.printf("[MOTOR] 移动到步数: theta=%ld, rho=%ld\n", thetaSteps, rhoSteps);
    return 0;
}

// ==================== 运动参数设置实现 ====================

int CommandExecutor::setDrawingSpeed(float speed) {
    if (!motionController) {
        Serial.println("[ERROR] MotionController未初始化");
        return 1;
    }
    
    motionController->setDrawingSpeed(speed);
    Serial.printf("[MOTOR] 绘图速度设置为: %.2f\n", speed);
    return 0;
}

int CommandExecutor::setAcceleration(float acceleration) {
    if (!motionController) {
        Serial.println("[ERROR] MotionController未初始化");
        return 1;
    }
    
    motionController->setAcceleration(acceleration);
    Serial.printf("[MOTOR] 加速度设置为: %.2f\n", acceleration);
    return 0;
}

int CommandExecutor::setCompensationFactor(float factor) {
    if (!motionController) {
        Serial.println("[ERROR] MotionController未初始化");
        return 1;
    }
    
    motionController->setCompensationFactor(factor);
    Serial.printf("[MOTOR] 补偿系数设置为: %.3f\n", factor);
    return 0;
}

int CommandExecutor::setRotationStepsPerRev(int steps) {
    if (!motionController) {
        Serial.println("[ERROR] MotionController未初始化");
        return 1;
    }
    
    motionController->setRotationStepsPerRev(steps);
    Serial.printf("[MOTOR] 旋转轴每圈步数设置为: %d\n", steps);
    return 0;
}

int CommandExecutor::setRadiusMaxSteps(int steps) {
    if (!motionController) {
        Serial.println("[ERROR] MotionController未初始化");
        return 1;
    }
    
    motionController->setRadiusMaxSteps(steps);
    Serial.printf("[MOTOR] 半径轴最大步数设置为: %d\n", steps);
    return 0;
}

// ==================== 电机线程控制实现 ====================

void CommandExecutor::motorControlTask(void* parameter) {
    CommandExecutor* executor = static_cast<CommandExecutor*>(parameter);
    
    Serial.println("[MOTOR_TASK] 电机控制线程开始运行");
    
    while (executor->motorTaskRunning) {
        if (xSemaphoreTake(executor->motorMutex, pdMS_TO_TICKS(10)) == pdTRUE) {
            if (executor->motionController) {
                executor->motionController->update();
                executor->motionController->checkMotorIdle();
            }
            xSemaphoreGive(executor->motorMutex);
        }
        
        vTaskDelay(pdMS_TO_TICKS(1));
    }
    
    Serial.println("[MOTOR_TASK] 电机控制线程结束");
    vTaskDelete(NULL);
}

bool CommandExecutor::startMotorControlTask() {
    if (motorTaskRunning) {
        Serial.println("⚠️ 电机控制线程已在运行");
        return true;
    }
    
    if (motorMutex == nullptr) {
        Serial.println("❌ 电机互斥锁未初始化");
        return false;
    }
    
    motorTaskRunning = true;
    
    BaseType_t result = xTaskCreatePinnedToCore(
        motorControlTask,
        "MotorControl",
        4096,
        this,
        configMAX_PRIORITIES - 1,
        &motorTaskHandle,
        1
    );
    
    if (result == pdPASS) {
        Serial.println("✅ 独立电机控制线程启动成功");
        Serial.println("🎯 电机将不受MQTT/SSL连接影响");
        return true;
    } else {
        motorTaskRunning = false;
        Serial.println("❌ 独立电机控制线程启动失败");
        return false;
    }
}

void CommandExecutor::stopMotorControlTask() {
    if (!motorTaskRunning) {
        Serial.println("⚠️ 电机控制线程未运行");
        return;
    }
    
    motorTaskRunning = false;
    
    if (motorTaskHandle != NULL) {
        vTaskDelete(motorTaskHandle);
        motorTaskHandle = NULL;
    }
    
    Serial.println("🛑 电机控制线程已停止");
}

bool CommandExecutor::isMotorTaskRunning() const {
    return motorTaskRunning;
}

// ==================== 电机管理命令实现 ====================

int CommandExecutor::executeAutoManageCommand(bool enabled) {
    motionController->enableAutoMotorManagement(enabled);
    Serial.printf("[MOTOR] 电机自动管理已%s\n", enabled ? "启用" : "禁用");
    return 0;
}

int CommandExecutor::executeTimeoutCommand(int timeoutSeconds) {
    if (timeoutSeconds < 0 || timeoutSeconds > 3600) {
        Serial.printf("[ERROR] 电机超时时间无效: %d秒 (范围: 0-3600秒)\n", timeoutSeconds);
        return 1;
    }
    
    unsigned long timeoutMs = timeoutSeconds * 1000;
    motionController->setMotorIdleTimeout(timeoutMs);
    Serial.printf("[MOTOR] 电机空闲超时时间已设置为: %d秒\n", timeoutSeconds);
    return 0;
}

int CommandExecutor::executePositionCommand() {
    if (!motionController) {
        Serial.println("[ERROR] 运动控制器未初始化");
        return 1;
    }
    
    Position pos = motionController->getCurrentPosition();
    Serial.println("=== 当前位置信息 ===");
    Serial.printf("角度 (theta): %.3f 弧度 (%.1f°)\n", pos.theta, pos.theta * 180.0 / PI);
    Serial.printf("半径 (rho): %.3f (0-1范围)\n", pos.rho);
    Serial.printf("旋转轴步数: %ld\n", pos.rotation_steps);
    Serial.printf("半径轴步数: %ld\n", pos.radius_steps);
    
    return 0;
}

// ==================== 流式扫图实现 ====================

int CommandExecutor::executeStreamSweepFromTHR() {
    Serial.println("🧹 开始流式扫图模式 (基于THR坐标数据)");
    
    if (!motionController) {
        Serial.println("❌ MotionController未初始化");
        return 2;
    }
    
    // 确保电机启用状态
    if (!motionController->areMotorsEnabled()) {
        motionController->enableMotors();
        delay(100);
        Serial.println("✅ 电机已启用");
    }
    
    // 设置扫图速度
    motionController->setMaxSpeed(400);
    Serial.println("✅ 扫图速度已设置为 400 步/秒");
    
    // 🔧 启动独立电机控制线程
    if (!isMotorTaskRunning()) {
        if (startMotorControlTask()) {
            Serial.println("🔧 独立电机控制线程已启动 (Core 1, 最高优先级)");
            Serial.println("✅ 独立电机控制线程启动成功");
            Serial.println("🎯 电机将不受MQTT/SSL连接影响");
        } else {
            Serial.println("⚠️ 扫图：电机控制线程启动失败，使用传统模式");
        }
    }
    
    // 设置绘图状态标志
    *isDrawingActive = true;
    
    // 在函数开始时设置绘图状态并启动LED会话
    setDrawingStateAndStartLED();
    
    if (ledControlCenter) {
        ledControlCenter->startDrawingSession();
        ledControlCenter->requestStatusChange(LED_DRAWING, "流式扫图开始");
    }
    
    // THR扫图数据点数组
    struct THRSweepPoint {
        float theta;  // 角度 (弧度)
        float rho;    // 半径 (0-1)
    };
    
    const THRSweepPoint thrSweepData[] = {
        // 螺旋线扫图：从外圈(rho=1.0)到圆心(rho≈0.044)，theta从0递增到约56弧度(约9圈)
        // 第1圈 (rho=1.0 -> 0.87)
        {0.0, 1.00}, {0.5, 0.99}, {1.0, 0.98}, {1.5, 0.97}, {2.0, 0.96}, {2.5, 0.95}, {3.0, 0.94}, {3.5, 0.93}, {4.0, 0.92}, {4.5, 0.91}, {5.0, 0.90}, {5.5, 0.89}, {6.0, 0.88}, {6.28, 0.87},
        // 第2圈 (rho=0.87 -> 0.74)
        {6.5, 0.86}, {7.0, 0.85}, {7.5, 0.84}, {8.0, 0.83}, {8.5, 0.82}, {9.0, 0.81}, {9.5, 0.80}, {10.0, 0.79}, {10.5, 0.78}, {11.0, 0.77}, {11.5, 0.76}, {12.0, 0.75}, {12.56, 0.74},
        // 第3圈 (rho=0.74 -> 0.61)
        {13.0, 0.73}, {13.5, 0.72}, {14.0, 0.71}, {14.5, 0.70}, {15.0, 0.69}, {15.5, 0.68}, {16.0, 0.67}, {16.5, 0.66}, {17.0, 0.65}, {17.5, 0.64}, {18.0, 0.63}, {18.5, 0.62}, {18.84, 0.61},
        // 第4圈 (rho=0.61 -> 0.48) 
        {19.0, 0.60}, {19.5, 0.59}, {20.0, 0.58}, {20.5, 0.57}, {21.0, 0.56}, {21.5, 0.55}, {22.0, 0.54}, {22.5, 0.53}, {23.0, 0.52}, {23.5, 0.51}, {24.0, 0.50}, {24.5, 0.49}, {25.12, 0.48},
        // 第5圈 (rho=0.48 -> 0.35)
        {25.5, 0.47}, {26.0, 0.46}, {26.5, 0.45}, {27.0, 0.44}, {27.5, 0.43}, {28.0, 0.42}, {28.5, 0.41}, {29.0, 0.40}, {29.5, 0.39}, {30.0, 0.38}, {30.5, 0.37}, {31.0, 0.36}, {31.40, 0.35},
        // 第6圈 (rho=0.35 -> 0.22)
        {32.0, 0.34}, {32.5, 0.33}, {33.0, 0.32}, {33.5, 0.31}, {34.0, 0.30}, {34.5, 0.29}, {35.0, 0.28}, {35.5, 0.27}, {36.0, 0.26}, {36.5, 0.25}, {37.0, 0.24}, {37.5, 0.23}, {37.68, 0.22},
        // 第7圈 (rho=0.22 -> 0.09)
        {38.0, 0.21}, {38.5, 0.20}, {39.0, 0.19}, {39.5, 0.18}, {40.0, 0.17}, {40.5, 0.16}, {41.0, 0.15}, {41.5, 0.14}, {42.0, 0.13}, {42.5, 0.12}, {43.0, 0.11}, {43.5, 0.10}, {43.96, 0.09},
        // 第8圈 (rho=0.09 -> 0.066)
        {44.0, 0.088}, {44.5, 0.086}, {45.0, 0.084}, {45.5, 0.082}, {46.0, 0.080}, {46.5, 0.078}, {47.0, 0.076}, {47.5, 0.074}, {48.0, 0.072}, {48.5, 0.070}, {49.0, 0.068}, {50.24, 0.066},
        // 第9圈 (rho=0.066 -> 0.044)
        {50.5, 0.064}, {51.0, 0.062}, {51.5, 0.060}, {52.0, 0.058}, {52.5, 0.056}, {53.0, 0.054}, {53.5, 0.052}, {54.0, 0.050}, {54.5, 0.048}, {55.0, 0.046}, {56.52, 0.044}
    };
    
    const int THR_SWEEP_POINTS = sizeof(thrSweepData) / sizeof(thrSweepData[0]);
    
    Serial.printf("📊 THR扫图数据点数: %d\n", THR_SWEEP_POINTS);
    Serial.println("🎯 开始执行流式THR扫图路径 (外圈→圆心)...");
    
    // 静态变量记录扫图进度（类似patternLoader的索引管理）
    static int sweepStartIndex = 0;
    
    // 如果是恢复扫图，从上次暂停的地方开始
    if (*isDrawingPaused && *pausedAtIndex > 0) {
        sweepStartIndex = *pausedAtIndex;
        *isDrawingPaused = false;
        Serial.printf("🔄 从扫图点 %d 恢复流式THR扫图\n", sweepStartIndex);
    } else {
        sweepStartIndex = 0; // 新的扫图从头开始
    }
    
    bool pauseRequested = false;  // 🔧 优雅暂停标志
    bool sweepCompleted = false;  // 🔧 标记扫图是否正常完成
    
    // 当前扫图索引（用于暂停恢复）
    int currentSweepIndex = sweepStartIndex;
    
    // 实现优化的流式扫图循环
    for (int i = sweepStartIndex; i < THR_SWEEP_POINTS && *isDrawingActive; i++) {
        currentSweepIndex = i; // 更新当前索引
        
        // 使用统一的LED更新函数
        updateLEDOptimized();
        
        // 在开始新点之前检查暂停请求
        if (pauseRequested) {
            Serial.println("⏸️ 完成当前扫图点后执行优雅暂停...");
            break;  // 退出扫图循环，触发暂停处理
        }
        
        // 移动到下一个扫图点
        THRSweepPoint point = thrSweepData[i];
        Serial.printf("🎯 扫图点 %d: θ=%.3f, ρ=%.3f\n", i+1, point.theta, point.rho);
        motionController->moveTo(point.theta, point.rho);
        
        // 在移动过程中等待移动完成 - 这是关键部分！
        while (motionController->isMovingNow() && *isDrawingActive && !pauseRequested) {
            // 🔧 混合模式：如果独立线程未运行，则在主线程中更新电机
            if (!isMotorTaskRunning()) {
                motionController->update();
            }
            
            // 优化：使用统一的高频LED更新
            updateLEDOptimized(false);
            
            // 检查串口输入
            if (Serial.available()) {
                static char cmdBuffer[32];
                size_t cmdLen = Serial.readBytesUntil('\n', cmdBuffer, sizeof(cmdBuffer) - 1);
                cmdBuffer[cmdLen] = '\0';
                
                // 简化版trim
                char* trimmed = cmdBuffer;
                while (*trimmed == ' ') trimmed++;
                
                if (strncmp(trimmed, "stop", 4) == 0 || strncmp(trimmed, "pause", 5) == 0) {
                    pauseRequested = true;
                    *pausedAtIndex = i;
                    Serial.println("📍 收到暂停请求，将在当前扫图点完成后暂停");
                }
            }
            
            // 处理网络任务
            if (networkTaskCallback != nullptr) {
                static unsigned long lastNetworkCall = 0;
                if (millis() - lastNetworkCall > 500) {
                    try {
                        networkTaskCallback();
                        lastNetworkCall = millis();
                    } catch (...) {
                        // 忽略网络任务错误
                    }
                }
            }
            
            vTaskDelay(pdMS_TO_TICKS(1));
        }
        
        // 检查是否因为暂停请求而退出移动循环
        if (pauseRequested) {
            break;
        }
    }
    
    // 处理扫图结束逻辑
    if (pauseRequested) {
        Serial.println("⏸️ 流式扫图被暂停");
        *isDrawingPaused = true;
        *pausedAtIndex = currentSweepIndex;
        setPausedState();
        return 1; // 返回暂停状态
    } else if (currentSweepIndex >= THR_SWEEP_POINTS - 1) {
        Serial.println("✅ 流式扫图正常完成");
        sweepCompleted = true;
        
        // 扫图完成的状态转换
        Serial.println("🔄 流式扫图阶段完成，准备进入绘图阶段");
        
        if (ledControlCenter) {
            ledControlCenter->requestStatusChange(LED_COMPLETED, "结束绘图会话");
            ledControlCenter->endDrawingSession();
        }
        
        // 重置绘图状态
        *isDrawingActive = false;
        *isDrawingPaused = false;
        *pausedAtIndex = -1;
        
        return 0; // 返回成功状态
    } else {
        Serial.println("❌ 流式扫图意外终止");
        return 2; // 返回错误状态
    }
}

// ==================== 辅助函数 ===========================

String CommandExecutor::getPositionInfoJson() {
    // 使用小型JSON缓冲区，位置信息不需要太大空间
    DynamicJsonDocument doc = createOptimizedJsonDoc(JSON_SIZE_SMALL);
    
    if (motionController) {
        Position pos = motionController->getCurrentPosition();
        doc["theta"] = pos.theta;
        doc["rho"] = pos.rho;
        doc["theta_degrees"] = pos.theta * 180.0 / PI;
        doc["rotation_steps"] = pos.rotation_steps;
        doc["radius_steps"] = pos.radius_steps;
        doc["is_moving"] = motionController->isMovingNow();
    } else {
        doc["error"] = FPSTR(MSG_CONTROLLER_ERROR);  // 使用PROGMEM常量
    }
    
    String result;
    STRING_RESERVE(result, 128);  // 预分配空间
    serializeJson(doc, result);
    return result;
}

String CommandExecutor::getMotionParametersJson() {
    // 使用中型JSON缓冲区，参数信息需要更多空间
    DynamicJsonDocument doc = createOptimizedJsonDoc(JSON_SIZE_MEDIUM);
    
    if (motionController) {
        doc["rotation_steps_per_rev"] = motionController->getRotationStepsPerRev();
        doc["radius_max_steps"] = motionController->getRadiusMaxSteps();
        doc["compensation_factor"] = motionController->getCompensationFactor();
        doc["max_speed"] = motionController->getMaxSpeed();
        doc["auto_management"] = motionController->isAutoMotorManagementEnabled();
        doc["idle_timeout_ms"] = motionController->getMotorIdleTimeout();
        doc["motors_enabled"] = motionController->areMotorsEnabled();
    } else {
        doc["error"] = FPSTR(MSG_CONTROLLER_ERROR);  // 使用PROGMEM常量
    }
    
    String result;
    STRING_RESERVE(result, 256);  // 预分配更大空间
    serializeJson(doc, result);
    return result;
}
